#include "../axis-tags/mainwindow.h"
#include <QApplication>

#include "Windmill/Windmill.h"
#include "Armor/EnemyDetector.h"
#include "Pose/AngleSolver.hpp"
#include "YOLOX/yolox.h"
#include<iostream>
#include<fstream>
#include<chrono>
#include<thread>
#include<mutex>
#include<unistd.h>

#define SHOW_VALUE_PLOT

using namespace std;
using namespace cv;
using namespace rm;


int main(int argc, char* argv[])
{
    QApplication a(argc, argv);
    std::unique_ptr<Windmill> _windmillPtr;
    _windmillPtr = make_unique<Windmill>();
    _windmillPtr->readFile();

    std::unique_ptr<AngleSolver> _solverPtr;
    _solverPtr = make_unique<AngleSolver>();
    AngleSolverParam angleParam;
    angleParam.readFile(13);//choose camera
    _solverPtr->init(angleParam);
    _solverPtr->setResolution(cv::Size(1280,1024));

    //ofstream outfile("/home/ssj5vkjcty/RoboMaster2021/2.txt",ios::app);

    int self_color;
    //self_color = rm::BLUE;
    self_color = rm::RED;
    _windmillPtr->setColor(self_color);

    EnemyDetector enemydetector;
    enemydetector.init();
    enemydetector.setEnemyColor(self_color == rm::BLUE ? rm::RED : rm::BLUE);

    int angleFlag;
    int armorType;
    std::vector<cv::Point2f> LightbarVertex;

    string filename = "/media/nuc/Pascal_t/RoboClub/SentryRobomaster2022/xiaotuoluo.avi";//打开的视频文件
    //string filename = "/home/ssj5vkjcty/图片/2.png";

    VideoCapture capture;
    capture.open(filename);
#ifdef SHOW_VALUE_PLOT
    MainWindow w;
    static double* ele_x = new double(0);
    static double* ele_y = new double(0);
    w.addElement(ele_x);
    w.addElement(ele_y);
#endif

    double rate = capture.get(CV_CAP_PROP_FPS);//获取视频文件的帧率
    int delay = cvRound(1000.000 / rate);

    if (!capture.isOpened())//判断是否打开视频文件
    {
        return -1;
    }
    else
    {
        auto startTime = chrono::high_resolution_clock::now();
        while (1)
        {
            auto start = chrono::high_resolution_clock::now();
            double timeStamp = (static_cast<chrono::duration<double,std::milli>>(chrono::high_resolution_clock::now() - startTime)).count();

            Mat frame;
            //            frame = imread(filename);
            capture >> frame;//读出每一帧的图像
            if (frame.empty()) break;

            enemydetector.loadImg(frame);
            if(enemydetector.detect())
            {
                LightbarVertex = enemydetector.getTargetVertex();
                armorType = enemydetector.getArmorType();
                for(int i = 0;i < LightbarVertex.size();++i){
                    cout << LightbarVertex[i] << endl;
                }

                cv::Point2f t_point=enemydetector.getTargetCenter();
                _solverPtr->setTarget(LightbarVertex,t_point, armorType);

                angleFlag = _solverPtr->solve();
                if(angleFlag != rm::AngleSolver::ANGLE_ERROR)
                {
                    double pnpDistance=_solverPtr->getDistance();
                    cout<<pnpDistance<<endl;

#ifdef SHOW_VALUE_PLOT
                    *ele_x = pnpDistance;
                    //                    *ele_y = enemydetector.getTargetCenter().y;
                    w.plot();
                    w.show();
#endif
                }
            }

            int flag = enemydetector.getArmorFlag();
            cout << "Flag:" << flag << endl;
            auto end = chrono::high_resolution_clock::now();
            auto tt = static_cast<chrono::duration<double, std::milli>>(end - start);
            std::cout << tt.count()<< "ms" << std::endl;
            waitKey(1);
        }
    }

    return 0;
}
